#include "iec61850_server.h"
#include "hal_thread.h"
#include <signal.h>
#include <errno.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <fcntl.h>
#include <string.h>
#include <pthread.h>
#include <arpa/inet.h>
#include <linux/un.h>
#include <sys/time.h>
#include <modbus.h>
#include <libxml/parser.h>
#include <libxml/xmlmemory.h>
#include "static_model.h"
#include "pcs_modbus_registers.h"
#include "pcs_server.h"
#if (SV_ENABLE == 1)
#include "sv_publisher.h"
#endif

#define TIMER_INIT 1
#define TIMER_REPEAT 5

#define UINT16_MAX 65535
#define READ_REGISTERS_MAX 125

//bitmap
enum {
    MODBUS_NONE = 0,
    MODBUS_READ = 0x1,
    MODBUS_WRITE = 0x2,
    MODBUS_UPDATE = 0x4,
    MODBUS_END
};

enum {
    TCP,
    TCP_PI,
    RTU,
    RS485
};

typedef struct{
    int registers_start;
    int registers_end;
    int registers_type;
    uint16_t *dataNew;
}modbus_registers_range;

#define REGISTERS_SIMULATOR_START 1000
#define REGISTERS_SIMULATOR_END 1078

#define REGISTERS_STATUS_START 1100
#define REGISTERS_STATUS_END 1107

#define REGISTERS_SYS1_START 3503
#define REGISTERS_SYS1_END 3594

#define REGISTERS_SYS2_START 6001
#define REGISTERS_SYS2_END 6043

#define REGISTERS_CONTROL_START 8000
#define REGISTERS_CONTROL_END 8002

#define REGISTERS_DEVICE_START 8100
#define REGISTERS_DEVICE_END 8100

static modbus_registers_range modbusRegisters[] = {
    {REGISTERS_SIMULATOR_START, REGISTERS_SIMULATOR_END, MODBUS_READ, NULL},
    {REGISTERS_STATUS_START   , REGISTERS_STATUS_END   , MODBUS_READ, NULL},
    {REGISTERS_SYS1_START     , REGISTERS_SYS1_END     , MODBUS_READ | MODBUS_WRITE, NULL},
    {REGISTERS_SYS2_START     , REGISTERS_SYS2_END     , MODBUS_READ | MODBUS_WRITE, NULL},
    {REGISTERS_DEVICE_START   , REGISTERS_DEVICE_END   , MODBUS_WRITE, NULL}
};

extern IedModel iedModel;
static IedServer iedServer = NULL;
static modbus_t *ctx = NULL;
static int running = 0;
void sigint_handler(int signal)
{
    running = 0;
}
//初始化寄存器
static void init_registers(void)
{
    int arrayNum = 0;
    int i = 0;
    int spaceSize = 0;
    arrayNum = sizeof(modbusRegisters) / sizeof(modbus_registers_range);

    for (i = 0; i <arrayNum; i++) {
        spaceSize = modbusRegisters[i].registers_end - modbusRegisters[i].registers_start + 1;
        spaceSize = spaceSize * sizeof(uint16_t);
        modbusRegisters[i].dataNew = (uint16_t *)malloc(spaceSize);
        memset(modbusRegisters[i].dataNew, 0, spaceSize);
    }
    return;
}

//初始化寄存器
static void uninit_registers(void)
{
    int arrayNum = 0;
    int i = 0;
    arrayNum = sizeof(modbusRegisters) / sizeof(modbus_registers_range);

    for (i = 0; i <arrayNum; i++) {
        free(modbusRegisters[i].dataNew);
        modbusRegisters[i].dataNew = NULL;
    }
    return;
}

//获取数据属性的浮点数数据属性
static DataAttribute* get_dataAttribute_f(DataAttribute* dataAttribute)
{
    DataAttribute* modelNode = dataAttribute;
    if (modelNode && (modelNode->type == IEC61850_CONSTRUCTED)) {
        modelNode = (DataAttribute*)modelNode->firstChild;
        while (modelNode) {
#ifdef DEBUG
                printf("Attribute name:%s\n", modelNode->name);
#endif
            if ((modelNode->type == IEC61850_FLOAT32) && (!strcmp(modelNode->name, "f"))) {
                break;
            }
            modelNode = (DataAttribute*)modelNode->sibling;
        }
    }
    return modelNode;
}

//获取时间数据属性
static DataAttribute* get_dataAttribute_t(ModelNode* dataObject)
{
    DataAttribute* modelNode = NULL;
    ModelNode* parentNode = dataObject;
    while (parentNode && (parentNode->modelType != DataObjectModelType)) {
        parentNode = parentNode->parent;
    }

    if (parentNode) {
        modelNode = (DataAttribute*)parentNode->firstChild;
        while (modelNode) {
            if ((modelNode->type == IEC61850_TIMESTAMP) && (!strcmp(modelNode->name, "t"))) {
                break;
            }
            modelNode = (DataAttribute*)modelNode->sibling;
        }
    }
    return modelNode;
}

//获取最大值和最小值
static DataAttribute* get_dataAttribute_range(DataAttribute* dataAttribute, float* min, float* max)
{
    float maxValue = 0.0;
    float minValue = 0.0;
    ModelNode* parentNode = NULL;
    DataAttribute* modelNode = NULL;
    DataAttribute* maxNode = NULL;
    DataAttribute* minNode = NULL;
    DataAttribute* qNode = NULL;

    parentNode = (ModelNode *)dataAttribute;
    while (parentNode && (parentNode->modelType != DataObjectModelType)) {
        parentNode = parentNode->parent;
    }

    if (parentNode) {
        modelNode = (DataAttribute*)parentNode->firstChild;
        while (modelNode) {
            if (!strcmp(modelNode->name, "maxVal")) {
                maxNode = modelNode;
            }
            if (!strcmp(modelNode->name, "minVal")) {
                minNode = modelNode;
            }
            if (!strcmp(modelNode->name, "q")) {
                qNode = modelNode;
            }
            modelNode = (DataAttribute*)modelNode->sibling;
        }
    }

    maxNode = get_dataAttribute_f(maxNode);
    minNode = get_dataAttribute_f(minNode);

    if (maxNode) {
        maxValue = maxNode->type == IEC61850_INT32 ? (float)MmsValue_toInt32(maxNode->mmsValue) : MmsValue_toFloat(maxNode->mmsValue);
    } else {
        maxValue = (float)(UINT16_MAX);
    }

    if (minNode) {
        minValue = minNode->type == IEC61850_INT32 ? (float)MmsValue_toInt32(minNode->mmsValue) : MmsValue_toFloat(minNode->mmsValue);
    } else {
        minValue = (float)(-UINT16_MAX);
    }

    if (maxValue == 0.0) {
        maxValue = (float)(UINT16_MAX);
    }
    if (minValue == 0.0) {
        minValue = (float)(-UINT16_MAX);
    }

    *max = maxValue;
    *min = minValue;
    return qNode;
}

//更新最大值和最小值
void set_dataAttribute_range(DataAttribute* dataAttribute, float min, float max)
{
    ModelNode* parentNode = NULL;
    DataAttribute* modelNode = NULL;
    DataAttribute* maxNode = NULL;
    DataAttribute* minNode = NULL;

    parentNode = (ModelNode *)dataAttribute;
    while (parentNode && (parentNode->modelType != DataObjectModelType)) {
        parentNode = parentNode->parent;
    }

    if (parentNode) {
        modelNode = (DataAttribute*)parentNode->firstChild;
        while (modelNode) {
            if (!strcmp(modelNode->name, "maxVal")) {
                maxNode = modelNode;
            }
            if (!strcmp(modelNode->name, "minVal")) {
                minNode = modelNode;
            }
            modelNode = (DataAttribute*)modelNode->sibling;
        }
    }

    maxNode = get_dataAttribute_f(maxNode);
    minNode = get_dataAttribute_f(minNode);

    if (maxNode) {
#ifdef DEBUG
        printf("Set maxVal\n");
#endif
        if (maxNode->type == IEC61850_INT32) {
            IedServer_updateInt32AttributeValue(iedServer, maxNode, (int)max);
        } else {
            IedServer_updateFloatAttributeValue(iedServer, maxNode, max);
        }
    }

    if (minNode) {
#ifdef DEBUG
        printf("Set minVal\n");
#endif
        if (minNode->type == IEC61850_INT32) {
            IedServer_updateInt32AttributeValue(iedServer, minNode, (int)min);
        } else {
            IedServer_updateFloatAttributeValue(iedServer, minNode, min);
        }
    }
    return;
}
//通过Addr，获取对应的发送数据属性结构体
RegisterDesc* get_registerDesc(int addr, int flag)
{
    int i = 0, j = 0;
    int addrStart = 0, addrEnd = 0;
    RegisterDesc *registerDesc = NULL;
    i = 0;
    while (modbusRegistersArray[i].addrStart != 0) {
        addrStart = modbusRegistersArray[i].addrStart;
        addrEnd = modbusRegistersArray[i].addrEnd;
        if ((addrStart <= addr) && (addr <= addrEnd)) {
            registerDesc = modbusRegistersArray[i].registerArray;

            j = 0;
            while (registerDesc[j].addr != 0) {
                if (registerDesc[j].addr == addr) {
                    return &registerDesc[j];
                }
                j++;
            }
        }
        i ++;
    }
    return registerDesc;
}
//获取相同功能约束以及名字的数据属性
static DataAttribute* get_dataAttribute_fc(ModelNode* dataObject, FunctionalConstraint fc, char *name)
{
    ModelNode* modelNode = NULL;
    DataAttribute* dataTmp = NULL;
    DataAttribute* dataAttribute = NULL;
    if (dataObject) {
        modelNode = modelNode->firstChild;
        while (modelNode) {
            if (modelNode->modelType == DataAttributeModelType) {
                dataTmp = (DataAttribute *)modelNode;
                if (dataTmp->fc == fc) {
                    if (name) {
                        if (!strcmp(dataTmp->name, name)) {
                            dataAttribute = (DataAttribute *)dataTmp;
                        } else if (dataTmp->type == IEC61850_CONSTRUCTED) {
                            dataAttribute = get_dataAttribute_fc((ModelNode *)dataTmp, fc, name);
                        }
                    } else {
                        dataAttribute = (DataAttribute *)dataTmp;
                    }
                }
            } else if (modelNode->modelType == DataObjectModelType) {
                dataAttribute = get_dataAttribute_fc(modelNode, fc, name);
            }

            if (!dataAttribute) {
                break;
            } else {
                modelNode = modelNode->sibling;
            }
            
        }
    }
    return dataAttribute;
}

static void iec_data_process(int addr, int data)
{
    int k = 0;
    ModelNode* modelNode = NULL;
    DataAttribute* dataPtr = NULL;
    DataAttribute* dataT = NULL;
    RegisterDesc *registerDesc = NULL;
    RegisterBitMap *bitMap = NULL;
    uint64_t timeStamp;
    uint16_t dataValue = 0;
    char* name = NULL;

#ifdef DEBUG
    printf("Addr[%d] modbus_data[%d]\n", addr, data);
#endif
#if (KEY_MAP_ENABLE == 1)
    if (regMap_isExist(addr)) {
        regMap_search_byValue(addr, data, &data);
#ifdef DEBUG
        printf("Addr[%d] ==> iec_data[%d]\n", addr, data);
#endif
    }
#endif
    registerDesc = get_registerDesc(addr, REG_RECV);
    if (registerDesc->registerBitMap == NULL) {
        modelNode = (ModelNode *)registerDesc->dataPtr;
        if (modelNode->modelType == DataObjectModelType) {
            name = registerDesc->scale == 1.0 ? "setVal" : "f";
#ifdef DEBUG
            printf("get_dataAttribute_fc name = %s\n", name);
#endif
            dataPtr = get_dataAttribute_fc(modelNode, IEC61850_FC_SP, name);
        } else if (modelNode->modelType == DataAttributeModelType) {
            dataPtr = (DataAttribute *)registerDesc->dataPtr;
        }

        if (!dataPtr) {
#ifdef DEBUG
            printf("ERROR: the DataAttribute point is NULL\n");
#endif
            return;
        }

        if (registerDesc->scale == 1.0) {
            IedServer_updateInt32AttributeValue(iedServer, dataPtr, data);
        } else {
            IedServer_updateFloatAttributeValue(iedServer, dataPtr, (float)(data * registerDesc->scale));
        }
        dataT = get_dataAttribute_t((ModelNode *)dataPtr);
        if (dataT) {
            timeStamp = Hal_getTimeInMs();
            IedServer_updateUTCTimeAttributeValue(iedServer, dataT, timeStamp);
        }
    } else {
        bitMap = registerDesc->registerBitMap;

        k = 0;
        while (bitMap[k].dataPtr != NULL) {
            dataValue =  (uint16_t)data;
            dataValue = dataValue >> bitMap[k].offset;
            dataValue = dataValue & (0xFFFF >> (16 - bitMap[k].nr));
            if (bitMap == switchInputStatus) {
                IedServer_updateInt32AttributeValue(iedServer, bitMap[k].dataPtr, dataValue);
            } else {
                IedServer_updateBooleanAttributeValue(iedServer, bitMap[k].dataPtr, (bool)dataValue);
            }
            
            dataT = get_dataAttribute_t((ModelNode *)registerDesc->dataPtr);
            if (dataT) {
                timeStamp = Hal_getTimeInMs();
                IedServer_updateUTCTimeAttributeValue(iedServer, dataT, timeStamp);
            }
            k ++;
        }
    }
    //特殊寄存器处理
    if (addr == 3513) {
        // registerDesc = get_registerDesc(3514, REG_SEND);
        // set_dataAttribute_range(registerDesc->dataPtr, (float)data / 1.73 * 0.95, (float)data / 1.73 * 1.05);
        reg3513_process(data);
    }

    if (addr == 3553) {
        reg3553_process(data);
    }
}

//modbus寄存器比对
static void modbus_register_cmp(int index, uint16_t *newData, int offset, int num)
{
    int size = 0;
    int i = 0;
    int addr_start = 0;
    size = modbusRegisters[index].registers_end - modbusRegisters[index].registers_start + 1;
    if (offset + num > size) {
#ifdef DEBUG
        printf("ERROR:compare out of range\n");
#endif
        return;
    }
    //寄存器缓存区起始地址
    addr_start = modbusRegisters[index].registers_start + offset;
    for (i = 0; i < num; i++) {
        if (modbusRegisters[index].dataNew[offset + i] != newData[i]) {
#ifdef DEBUG
            printf("Update:addr=%d new(%d)==>old(%d)\n", addr_start + i, newData[i], modbusRegisters[index].dataNew[offset + i]);
#endif
            modbusRegisters[index].dataNew[offset + i] = newData[i];
            iec_data_process(addr_start + i, newData[i]);
        }
    }
}

static void timer_signal_handler(int sig)
{
    unsigned int arrayNum = 0;
    unsigned int i = 0;
    unsigned int registerNum = 0;
    int readNum = 0;
    unsigned int addrBegin = 0;
    unsigned int addrOffset = 0;
    uint16_t *dataPtr = NULL;
    arrayNum = sizeof(modbusRegisters) / sizeof(modbus_registers_range);
    if (sig == SIGALRM) {
        dataPtr = (uint16_t *)malloc(READ_REGISTERS_MAX * sizeof(uint16_t));

        for (i = 0; i <arrayNum; i++) {
            registerNum = modbusRegisters[i].registers_end - modbusRegisters[i].registers_start + 1;
            addrBegin = modbusRegisters[i].registers_start;
            addrOffset = 0;
            while(registerNum) {
                if (registerNum > READ_REGISTERS_MAX) {
                    readNum = READ_REGISTERS_MAX;
                } else {
                    readNum = registerNum;
                }
                memset(dataPtr, 0, READ_REGISTERS_MAX * sizeof(uint16_t));
                readNum = modbus_read_registers(ctx, addrBegin, readNum, dataPtr);
                if (readNum == -1) {
#ifdef DEBUG
                printf("ERROR: Read error return %d\n", readNum);
#endif
                    break;
                }
                //对比新旧数据
                modbus_register_cmp(i, dataPtr, addrOffset, readNum);

                addrBegin += readNum;
                registerNum -= readNum;
                addrOffset += readNum;
            }
        }
        free(dataPtr);
        dataPtr = NULL;
    }
}
//设置定时器
static void setup_timer(int seconds, int repeat) {
    struct itimerval new_value, old_value;
    // 设置定时器初始值
    new_value.it_value.tv_sec = seconds;
    new_value.it_value.tv_usec = 0;
    // 设置定时器停止后的状态，0表示不重复
    new_value.it_interval.tv_sec = repeat;
    new_value.it_interval.tv_usec = 0;
    // 设置定时器类型为ITIMER_REAL
    setitimer(ITIMER_REAL, &new_value, &old_value);
}

static void connectionHandler (IedServer self, ClientConnection connection, bool connected, void* parameter)
{
    if (connected)
        printf("Connection opened\n");
    else
        printf("Connection closed\n");
}

static MmsDataAccessError writeAccessHandler (DataAttribute* dataAttribute, MmsValue* value, ClientConnection connection, void* parameter)
{
    float val = 0;
    float max = 0, min = 0;
    int rc = 0;
    int data = 0;
    ModelNode* dataNode = NULL;
    DataAttribute* dataPtr = NULL;
    DataAttribute* dataT = NULL;
    RegisterDesc *regDesc = NULL;
    DataAttribute* qNode = NULL;
    uint64_t timeStamp;

    if ((dataAttribute == NULL) || (value == NULL) || (parameter == NULL)) {
        return DATA_ACCESS_ERROR_UNKNOWN;
    }

    regDesc = (RegisterDesc *) parameter;
    dataNode = (ModelNode *)regDesc->dataPtr;
    if (dataNode->modelType == DataAttributeModelType) {
        dataPtr = dataAttribute;
    } else if(dataNode->modelType == DataObjectModelType) {
        
        if (!strcmp(dataAttribute->name, "setVal")) {
            dataPtr = dataAttribute;
        } else if(!strcmp(dataAttribute->name, "f")) {
            dataPtr = dataAttribute;
        } else {
            return DATA_ACCESS_ERROR_OBJECT_ACCESS_UNSUPPORTED;
        }
    }

    //找出当前数据属性的最大值最小值
    qNode = get_dataAttribute_range(dataPtr, &min, &max);
    //值范围比较
    if (regDesc->scale == 1.0) {
        val = (float)MmsValue_toInt32(value);
    } else {
        val = MmsValue_toFloat(value);
    }
    
#ifdef DEBUG
    printf("MIN:%f val:%f MAX:%f dataPtr:%ld\n", min, val, max, (long int)qNode);
#endif
    if ((val < min) || (val > max)) {
        if (qNode) {
            IedServer_updateQuality(iedServer, qNode, QUALITY_VALIDITY_QUESTIONABLE | QUALITY_DETAIL_OUT_OF_RANGE);
        }
        return DATA_ACCESS_ERROR_SUCCESS_NO_UPDATE;
    }
    val = val / regDesc->scale;
    data = (int)val;
#ifdef DEBUG
    printf("Addr:%d scale:%f value:%f dataPtr:%ld\n", regDesc->addr, regDesc->scale, val, (long int)regDesc->dataPtr);
#endif

//对枚举类型寄存器进行值有效性判断
#if (KEY_MAP_ENABLE == 1)
    bool ret = false;
    if (regMap_isExist(regDesc->addr)) {
        ret = regMap_search_byValue(regDesc->addr, data, &data);
        if (!ret) {
            return DATA_ACCESS_ERROR_TEMPORARILY_UNAVAILABLE;
        }
#ifdef DEBUG
        printf("Addr[%d] ==> modbus_data[%d]\n", regDesc->addr, data);
#endif
    }
#endif

    //对特殊寄存器检查检查，不通过检查返回-1
    rc = modbus_write_register(ctx, regDesc->addr, (uint16_t)data);
    if (rc == -1) {
        if (qNode) {
            IedServer_updateQuality(iedServer, qNode, QUALITY_VALIDITY_QUESTIONABLE | QUALITY_DETAIL_FAILURE);
        }
    }

    //特殊寄存器处理，关联性寄存器
    if (regDesc->addr == 3513) {
        // RegisterDesc *registerDesc = get_registerDesc(3514, REG_SEND);
        // set_dataAttribute_range(registerDesc->dataPtr, (float)data / 1.73 * 0.95, (float)data / 1.73 * 1.05);
        reg3513_process(data);
    }

    if (regDesc->addr == 3553) {
        reg3553_process(data);
    }

#ifdef DEBUG
    printf("addr:%d value:%d\n", regDesc->addr, (uint16_t)data);
    printf("Write register result\n");
#endif
    if (rc != 1) {
        return DATA_ACCESS_ERROR_HARDWARE_FAULT;
    }
    dataT = get_dataAttribute_t((ModelNode *)dataPtr);
    if (dataT) {
        timeStamp = Hal_getTimeInMs();
        IedServer_updateUTCTimeAttributeValue(iedServer, dataT, timeStamp);
    }
    return DATA_ACCESS_ERROR_SUCCESS;
}

static void init_writeHandler(void)
{
    int i = 0, j =0;
    ModelNode* dataNode = NULL;
    RegisterDesc *sendArray = NULL;
#ifdef DEBUG
    printf("Enter: %s: %d\n", __FILE__, __LINE__);
#endif
    
    i = 0;
    while(modbusRegistersArray[i].addrStart != 0) {
        if (modbusRegistersArray[i].flag & REG_SEND) {
#ifdef DEBUG
        printf("Start=%d End=%d Flag=%d RegArray=%ld\n", 
                modbusRegistersArray[i].addrStart,
                modbusRegistersArray[i].addrEnd,
                modbusRegistersArray[i].flag,
                (long int)modbusRegistersArray[i].registerArray);
#endif
            sendArray = modbusRegistersArray[i].registerArray;
            j = 0;
            while(sendArray[j].addr != 0){
                dataNode = (ModelNode *)sendArray[j].dataPtr;
                /* Install write callback handler for substitution variables */
                if (dataNode->modelType == DataAttributeModelType) {
                    IedServer_handleWriteAccess(iedServer, (DataAttribute *)sendArray[j].dataPtr, writeAccessHandler, &sendArray[j]);
                } else if (dataNode->modelType == DataObjectModelType) {
                    IedServer_handleWriteAccessForDataObject(iedServer, (DataObject *)sendArray[j].dataPtr, IEC61850_FC_SP, writeAccessHandler, &sendArray[j]);
                }
                j ++;
            }
        }
        i++;
    }

}

static void goCbEventHandler(MmsGooseControlBlock goCb, int event, void* parameter)
{
    printf("Access to GoCB: %s\n", MmsGooseControlBlock_getName(goCb));
    printf("         GoEna: %i\n", MmsGooseControlBlock_getGoEna(goCb));
}

#if (SV_ENABLE == 1)
static SVPublisher svPublisher = NULL;
static SVPublisher_ASDU asdu;
static int svcbEnabled = SVCBENABLE;
static int fIndex1 = 0;
static int sampleCount = 0;
static void sVCBEventHandler (SVControlBlock* svcb, int event, void* parameter)
{
    if (event == IEC61850_SVCB_EVENT_ENABLE)
        svcbEnabled = 1;
    else if (event == IEC61850_SVCB_EVENT_DISABLE)
        svcbEnabled = 0;
}
void init_sv_server(IedServer iedServer, IedModel *model)
{
    int i = 0;
    CommParameters svParameter;
    PhyComAddress *svPhyCom = model->svCBs->dstAddress;
    svParameter.vlanPriority = svPhyCom->vlanPriority;
    svParameter.vlanId = svPhyCom->vlanId;
    svParameter.appId = svPhyCom->appId;
    for (i = 0; i < 6; i++) {
        svParameter.dstAddress[i] = svPhyCom->dstAddress[i];
    }
    
    svPublisher = SVPublisher_create(&svParameter, SV_ETH_INTERFACE);
    if (svPublisher) {
        asdu = SVPublisher_addASDU(svPublisher, SVID_NAME, NULL, 1);
        fIndex1 = SVPublisher_ASDU_addFLOAT(asdu);

        SVPublisher_ASDU_setSmpCntWrap(asdu, SV_SAMPLE_LIMIT);
        SVPublisher_ASDU_setRefrTm(asdu, 0);
        SVPublisher_setupComplete(svPublisher);
    }

    if (svPublisher) {
        SVControlBlock* svcb = IedModel_getSVControlBlock(model, &iedModel_Sta_LLN0, SVCB_NAME);
        if (svcb == NULL) {
            printf("Lookup svcb failed!\n");
            exit(1);
        }
        IedServer_setSVCBHandler(iedServer, svcb, sVCBEventHandler, NULL);
    }
}
void sv_sample_publisher(void)
{
    if (svcbEnabled && svPublisher) {
        SVPublisher_ASDU_setFLOAT(asdu, fIndex1, sampleCount);

        SVPublisher_ASDU_setRefrTmNs(asdu, Hal_getTimeInNs());
        SVPublisher_ASDU_setSmpCnt(asdu, (uint16_t) sampleCount);
        SVPublisher_publish(svPublisher);
    }
    sampleCount = ((sampleCount + 1) % SV_SAMPLE_LIMIT);
}
#endif

int init_iec_server()
{
    printf("Using libIEC61850 version %s\n", LibIEC61850_getVersionString());
    /* Create a new IEC 61850 server instance */
    iedServer = IedServer_create(&iedModel);
#if (GOOSE_CB_ENABLE == 1)
    /* set GOOSE interface for all GOOSE publishers (GCBs) */
    IedServer_setGooseInterfaceId(iedServer, GOOSE_ETH_INTERFACE);
    /* set GOOSE interface for a particular GOOSE publisher (GCB) */
    //IedServer_setGooseInterfaceIdEx(iedServer, &iedModel_Sta_LLN0, "DCGcbValues", GOOSE_ETH_INTERFACE);
    IedServer_setGoCBHandler(iedServer, goCbEventHandler, NULL);
#endif
    IedServer_setConnectionIndicationHandler(iedServer, (IedConnectionIndicationHandler) connectionHandler, NULL);
    init_writeHandler();
    /* MMS server will be instructed to start listening for client connections. */
    IedServer_start(iedServer, 10002);

    if (!IedServer_isRunning(iedServer)) {
        printf("Starting server failed! Exit.\n");
        IedServer_destroy(iedServer);
        exit(-1);
    }
#if (GOOSE_CB_ENABLE == 1)
    /* Start GOOSE publishing */
    IedServer_enableGoosePublishing(iedServer);
#endif
#if (SV_ENABLE == 1)
    init_sv_server(iedServer, &iedModel);
#endif
    running = 1;
    return 0;
}

int init_modbus_port(int argc, char *argv[])
{
    #define SERVER_ID 17
    unsigned sec;
    unsigned usec;

    int use_backend;
    char *ip_or_device;
    if (argc > 1) {
        if (strcmp(argv[1], "tcp") == 0) {
            use_backend = TCP;
        } else if (strcmp(argv[1], "tcppi") == 0) {
            use_backend = TCP_PI;
        } else if (strcmp(argv[1], "rtu") == 0) {
            use_backend = RTU;
        } else if (strcmp(argv[1], "rs485") == 0) {
            use_backend = RS485;
        } else {
            printf("Modbus client for unit testing\n");
            printf("Usage:\n  %s [tcp|tcppi|rtu]\n", argv[0]);
            printf("Eg. tcp 127.0.0.1 or rtu /dev/ttyUSB1\n\n");
            exit(1);
        }
    } else {
        /* By default */
        use_backend = TCP;
    }

    if (argc > 2) {
        ip_or_device = argv[2];
    } else {
        switch (use_backend) {
        case TCP:
            ip_or_device = "127.0.0.1";
            break;
        case TCP_PI:
            ip_or_device = "::1";
            break;
        case RTU:
            ip_or_device = argv[2];
        case RS485:
            ip_or_device = argv[2];
            break;
        default:
            break;
        }
    }

    if (use_backend == TCP) {
        ctx = modbus_new_tcp(ip_or_device, 1502);
    } else if (use_backend == TCP_PI) {
        ctx = modbus_new_tcp_pi(ip_or_device, "1502");
    } else if (use_backend == RTU) {
        ctx = modbus_new_rtu(ip_or_device, 9600, 'N', 8, 1);
    }  else if (use_backend == RS485) {
        ctx = modbus_new_rtu_rs485(ip_or_device, 9600, 'N', 8, 1);
    }
    if (ctx == NULL) {
        fprintf(stderr, "Unable to allocate libmodbus context\n");
        return -1;
    }

    //modbus_set_debug(ctx, TRUE);
    modbus_set_error_recovery(ctx, MODBUS_ERROR_RECOVERY_LINK | MODBUS_ERROR_RECOVERY_PROTOCOL);

    if ((use_backend == RTU) || (use_backend == RS485)) {
        modbus_set_slave(ctx, SERVER_ID);
    }

    modbus_get_response_timeout(ctx, &sec, &usec);
    if (modbus_connect(ctx) == -1) {
        fprintf(stderr, "Connection failed: %s\n", modbus_strerror(errno));
        modbus_free(ctx);
        ctx = NULL;
        return -1;
    }
    return 0;
}

//同步PCS时间
int sync_time_pcs(void)
{
#define TIME_REGISTER_ADDR 7501
#define TIME_REGISTER_NUM 6
    int rc = 0;
    int64_t timeStamp;
    uint16_t timeRegisterTab[TIME_REGISTER_NUM] = {0};
    timeStamp = Hal_getTimeInMs();
    struct tm *pcsTime = localtime(&timeStamp);
#ifdef DEBUG
    printf("%d-%02d-%02d %02d:%02d:%02d\n",
           pcsTime->tm_year + 1900,
           pcsTime->tm_mon + 1,
           pcsTime->tm_mday,
           pcsTime->tm_hour,
           pcsTime->tm_min,
           pcsTime->tm_sec);
#endif
    timeRegisterTab[0] = (uint16_t)(pcsTime->tm_year + 1900);
    timeRegisterTab[1] = (uint16_t)(pcsTime->tm_mon + 1);
    timeRegisterTab[2] = (uint16_t)(pcsTime->tm_mday);
    timeRegisterTab[3] = (uint16_t)(pcsTime->tm_hour);
    timeRegisterTab[4] = (uint16_t)(pcsTime->tm_min);
    timeRegisterTab[5] = (uint16_t)(pcsTime->tm_sec);
    /* Many registers */
    rc = modbus_write_registers(ctx, TIME_REGISTER_ADDR, TIME_REGISTER_NUM, timeRegisterTab);
#ifdef DEBUG
    if (rc != TIME_REGISTER_NUM) {
        printf("Update PCS time failed\n");
    }
#endif
    return rc;
}

int main(int argc, char *argv[])
{
    int ret = 0;
    //初始化modbus port
    ret = init_modbus_port(argc, argv);
    if (ret == -1) {
        printf("Init modbus port failed\n");
    }
    //初始化定时器
    setup_timer(TIMER_INIT, TIMER_REPEAT);
    signal(SIGALRM,timer_signal_handler);
    signal(SIGINT, sigint_handler);
    init_registers();
    sync_time_pcs();
    //初始化IEC61850 server
    init_iec_server();
    while(running) {
        Thread_sleep(100);
#if (SV_ENABLE == 1)
        sv_sample_publisher();
#endif
    }
    /* stop MMS server - close TCP server socket and all client sockets */
    IedServer_stop(iedServer);
#if (SV_ENABLE == 1)
        SVPublisher_destroy(svPublisher);
#endif
    /* Cleanup - free all resources */
    IedServer_destroy(iedServer);
    uninit_registers();
    return 0;
}
